#include <ros/ros.h>
#include <shared_memory_communicator/shared_memory_communicator.hpp>
#include <std_msgs/Header.h>

using namespace std;

int main( int argc, char** argv ) {
    ros::init( argc, argv, "test_node" );
    ros::NodeHandle nh;

    SharedMemoryROSCommunicator< std_msgs::Header > cppCommunicator( "cpp_server" );
    SharedMemoryROSCommunicator< std_msgs::Header > pythonCommunicator( "python_server" );

    std_msgs::Header header;
    header.frame_id = "C++";
    header.stamp = ros::Time::now();

    while ( ros::ok() ) {
        header.stamp = ros::Time::now();
        cppCommunicator.publish( header );
        auto recv = pythonCommunicator.read();
        cout << recv << endl;
        ros::Duration( 0.5 ).sleep();
    }
}
